package recolhalixo;

import jadex.extension.envsupport.environment.ISpaceObject;
import jadex.extension.envsupport.environment.space2d.Grid2D;
import jadex.extension.envsupport.math.IVector2;
import jadex.extension.envsupport.math.Vector2Int;
import jadex.extension.envsupport.observer.graphics.drawable.DrawableCombiner;

import java.util.ArrayList;
import java.util.Collection;
import java.util.LinkedList;
import java.util.Stack;

public class PathFinder {
	
	private static class EstadoPesquisa {
		public IVector2 pos;
		public LinkedList<IVector2> path;
		public EstadoPesquisa(IVector2 pos,LinkedList<IVector2> path) {
			this.pos=pos;
			this.path=path;
		}
	}
	
	public static LinkedList<IVector2> findPath(IVector2 myPos,ArrayList<IVector2> goals,Grid2D env) {
		
		if (goals.isEmpty()) {return new LinkedList<IVector2>();}
		LinkedList<IVector2> path=new LinkedList<IVector2>();
		
		LinkedList<EstadoPesquisa> nos=new LinkedList<EstadoPesquisa>();
		
		nos.push(new EstadoPesquisa(myPos, new LinkedList<IVector2>()));
		while (!nos.isEmpty()) {
			EstadoPesquisa estado=nos.pop();
			IVector2 thisPos=estado.pos;
			LinkedList<IVector2> caminho=(LinkedList<IVector2>) estado.path.clone();
			caminho.add(thisPos);
			if (goals.contains(thisPos)) {path=caminho;break;}
			int px=thisPos.getXAsInteger();
			int py=thisPos.getYAsInteger();
			
			Collection temp=env.getSpaceObjectsByGridPosition(thisPos, "estrada");
			Collection temp_central=env.getSpaceObjectsByGridPosition(thisPos, "central");
			if (temp!=null || temp_central!=null) {
				
				Vector2Int pCima=new Vector2Int(px,py-1);
				Vector2Int pBaixo=new Vector2Int(px,py+1);
				Vector2Int pEsquerda=new Vector2Int(px-1,py);
				Vector2Int pDireita=new Vector2Int(px+1,py);
				String dir="";
				if (temp_central==null) {
					dir=(String)((ISpaceObject)temp.iterator().next()).getProperty("direction");
				}
				else {dir="udlr";}
				if (dir.contains("u") && env.getSpaceObjectsByGridPosition(pCima, "estrada")!=null) {
					nos.addLast(new EstadoPesquisa(pCima, caminho));
				}
				if (dir.contains("d") && env.getSpaceObjectsByGridPosition(pBaixo, "estrada")!=null) {
					nos.addLast(new EstadoPesquisa(pBaixo, caminho));
				}
				if (dir.contains("l") && env.getSpaceObjectsByGridPosition(pEsquerda, "estrada")!=null) {
					nos.addLast(new EstadoPesquisa(pEsquerda, caminho));
				}
				if (dir.contains("r") && env.getSpaceObjectsByGridPosition(pDireita, "estrada")!=null) {
					nos.addLast(new EstadoPesquisa(pDireita, caminho));
				}
			}
		}
		return path;
	}
}
